#include <stdio.h>
#include <math.h>
#include <vector>
#include "monotone_cubic.hpp"
#include "hermite/hermite_cubic.hpp"
using namespace std;
namespace math
{
	namespace monotone_cubic
	{

		double interp_val(
			long samp_n, double samp_x[], double samp_y[], double samp_div[],
			long c1s_n, double c1s[], long c2s_n, double c2s[], long s3s_n, double c3s[],
			double x);

		bool monotone_cubic_interp(
			long samp_n, double samp_x[], double samp_y[],
			long n, double x[], double y[])
		{
			if (samp_n == 0) return false;
			if (samp_n == 1) return false;
		
			// Get consecutive differences and slopes

			double * dys = new double[samp_n];
			double * dxs = new double[samp_n];
			double * ms = new double[samp_n];
			for (long i = 0; i < samp_n - 1; i++) 
			{
				dxs[i] = samp_x[i + 1] - samp_x[i];
				dys[i] = samp_y[i + 1] - samp_y[i];
				ms[i] = dys[i] / dxs[i];
			}

			// Get degree-1 coefficients
			double * c1s = new double[samp_n];
			double * c2s = new double[samp_n];
			double * c3s = new double[samp_n];
			for (long i = 0; i < samp_n - 1; i++) 
			{
				double m = ms[i], mNext = ms[i + 1];
				if (m*mNext <= 0) 
				{
					c1s[i] = 0;
				}
				else 
				{
					double dx = dxs[i], dxNext = dxs[i + 1], common = dx + dxNext;
					c1s[i] = (3 * common / ((common + dxNext) / m + (common + dx) / mNext));
				}
			}
			c1s[samp_n-1] = ms[samp_n - 1 -1];

			// Get degree-2 and degree-3 coefficients
			for (long i = 0; i < samp_n-1; i++) 
			{
				double c1 = c1s[i], 
					m = ms[i], 
					invDx = 1 / dxs[i], 
					common = c1 + c1s[i + 1] - m - m;
				c2s[i] = (m - c1 - common)*invDx; 
				c3s[i] = common*invDx*invDx;
			}

			for (long i = 0; i < n; i++)
			{
				y[i] = interp_val(samp_n, samp_x, samp_y, ms,
					samp_n, c1s, samp_n, c2s, samp_n, c3s, x[i]);
			}
			return true;
		}
		
		// Return interpolant function
		double interp_val(
			long samp_n, double samp_x[], double samp_y[], double samp_div[], 
			long c1s_n, double c1s[], long c2s_n, double c2s[], long s3s_n, double c3s[],
			double x )
		{
			// The rightmost point in the dataset should give an exact result
			long i = samp_n - 1;
			if (x == samp_x[i]) return samp_y[i]; 

			// Search for the interval x is in, returning the corresponding y if x is one of the original xs
			long low = 0, mid, high = s3s_n - 1;
			while (low <= high) {
				mid = long(floor(0.5*(low + high)));
				double xHere = samp_x[mid];
				if (xHere < x) 
				{ 
					low = mid + 1; 
				}
				else if (xHere > x) 
				{
					high = mid - 1;
				}
				else 
				{ 
					return samp_y[mid]; 
				}
			}
			i = __max(0, high);
			// Interpolate
			double diff = x - samp_x[i], diffSq = diff*diff;
			return samp_y[i] + c1s[i] * diff + c2s[i] * diffSq + c3s[i] * diff*diffSq;
		};





	}
}